#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

String buffer;
int ServoNum = 0, PulseNum = 0;
const float MaxPluse = 600,MinPluse=150;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

void setServoAngle(int channel, int angle) {
  // 将角度转换为 PWM 脉冲宽度
  int pulse = map(angle, 0, 180, MinPluse, MaxPluse); // 150-600 对应 0-180 度
  pwm.setPWM(channel, 0, pulse); // 设置通道的 PWM 脉冲宽度
}

void ServoControl(int id, int angle)
{
  Serial.println(angle);
  switch (id)
  {
  case 0:
    setServoAngle(0, angle);
    break;
  case 1:
    setServoAngle(1, angle);
    break;
  case 2:
    setServoAngle(2, angle);
    break;
  case 3:
    setServoAngle(3, angle);
    break;
  case 4:
    setServoAngle(4, angle);
    break;
  case 5:
    setServoAngle(5, angle);
    break;
  case 6:
    setServoAngle(6, angle);
    break;
  case 7:
    setServoAngle(7, angle);
    break;
  case 8:
    setServoAngle(8, angle);
    break;
  case 9:
    setServoAngle(9, angle);
    break;
  case 10:
    setServoAngle(10, angle);
    break;
  case 11:
    setServoAngle(11, angle);
    break;
  case 12:
    setServoAngle(12, angle);
    break;
  case 13:
    setServoAngle(13, angle);
    break;
  case 14:
    setServoAngle(14, angle);
    break;
  case 15:
    setServoAngle(15, angle);
    break;
  default:
    break;
  }
}

void setup()
{
  Serial.begin(9600); // 初始化串行通信
  Serial.println("test");

  pwm.begin();
  pwm.setPWMFreq(50); // 设置PWM频率为50Hz
}

void loop()
{
  if(Serial.available()>0){
    while (true){
      int data = Serial.read();
      if(data == '#'){
        break;
      }
    }
    buffer=Serial.readStringUntil('P');
    ServoNum=buffer.toInt();
    Serial.println(ServoNum);

    while(Serial.available()==0) delay(1);

    buffer=Serial.readStringUntil('T');
    PulseNum=buffer.toInt();
    Serial.println(PulseNum);
    
    ServoControl(ServoNum, PulseNum);
    delay(1);
  }
  delay(1);
}
